#include "EntityQuadTree.h"

EntityQuadTree 	EntityQuadTree::root;
std::list<Entity*>	EntityQuadTree::updateQueue;

EntityQuadTree::EntityQuadTree() : QuadTree(){


}
void EntityQuadTree::InitializeRoot(){

    
    root.SetNodeCenter(WIDTH/2, HEIGHT/2);
    root.SetNodeDimensions(WIDTH, HEIGHT);
  /*  for(int i = 0; i < QUADTREE_LEVELS; i++)
        root.DivideLeaves();*/
}
EntityQuadTree::~EntityQuadTree(){}
void EntityQuadTree::AddEntity(Entity* entity){

    //find the lowest node which can contain the entity
    EntityQuadTree* eqt_node = GetNodeSurroundingRectangle(entity->GetBody()->GetDimensions());

	//add the entity to entities[] of that node
    eqt_node->PushEntityToCollection(entity);


}
void EntityQuadTree::PushEntityToCollection(Entity* entity){
    entities.push_back(entity);
}
int EntityQuadTree::GetEntityCountInNode(){
    return entities.size();
}
Entity* EntityQuadTree::GetEntityAtNode(unsigned int i){
    if(i < entities.size())
        return entities.at(i);
    else
        return NULL;
}
void EntityQuadTree::PerformEntityCollisionSweep(Entity* entity){

	for(int i = 0; i < GetEntityCountInNode(); i++){
		if(entities.at(i) != entity)
			entity->CollideWith(entities.at(i));
		
	}

	if(IsLeaf())
        return;
	

	Rectangle* rect = entity->GetBody()->GetDimensions();

    for(int i = 0; i < 4; i++)
        if(branches[i]->areaAssignedToNode.AreCoordinatesWithinShape(rect->GetX()-rect->GetHalfOfWidth(), rect->GetY()-rect->GetHalfOfHeight())
            && branches[i]->areaAssignedToNode.AreCoordinatesWithinShape(rect->GetX()+rect->GetHalfOfWidth(), rect->GetY()+rect->GetHalfOfHeight()))
		{
			((EntityQuadTree*)branches[i])->PerformEntityCollisionSweep(entity);
			
		}

	
	
    
		
}
void EntityQuadTree::TrimNodes(){
	////doesn't work
	//for(int i = 0; i < 4; i++){
	//	if(!IsLeaf()){
	//		//penultimate node
	//		if(((EntityQuadTree*)branches[i])->IsLeaf()){
	//			int entity_count = 0;
	//			for(int j = 0; j < 4; j++)
	//				entity_count += ((EntityQuadTree*)branches[j])->GetEntityCountInNode();
	//			if(entity_count == 0){						
	//				UniteLeafNodes();
	//			}
	//		}
	//		else
	//			((EntityQuadTree*)branches[i])->TrimNodes();
	//	}
	//}
}
void EntityQuadTree::UpdateNode(){
	
	
	SetChanged(true);
	//determine whether the current node is full
	//subdivide if possible
	if(GetEntityCountInNode() > MAXIMUM_ENTITIES_PER_NODE && areaAssignedToNode.GetW() > MINIMUM_NODE_SIZE){
		for(int i = 0; i < entities.size(); i++)
			updateQueue.push_back(entities.at(i));
		entities.clear();
		DivideLeaves();
	}


	if(HasChanged()){
		for(int i = 0; i < entities.size(); i++)
			updateQueue.push_back(entities.at(i));
		entities.clear();
	}	
	//recurse
	if(!IsLeaf()){
		for(int i = 0; i < 4; i++)
			((EntityQuadTree*)branches[i])->UpdateNode();

		
	}
}
void EntityQuadTree::AddPendingEntities(){
	if(updateQueue.size() > 0){
		//remove entities from the current node and add them to the appropriate nodes
		std::list<Entity*>::iterator iter = updateQueue.begin();
		while(iter != updateQueue.end()){
			AddEntity((*iter));
			iter++;
			updateQueue.erase(updateQueue.begin());
		}
	}
	
}
EntityQuadTree* EntityQuadTree::GetNodeAtPoint(unsigned int x, unsigned int y){
    if(IsLeaf())
        return this;

    for(int i = 0; i < 4; i++)
        if(branches[i]->PointIsInsideNode(x,y))
            return (EntityQuadTree*)branches[i]->GetNodeAtPoint(x,y);

}
EntityQuadTree* EntityQuadTree::GetNodeSurroundingRectangle(Rectangle* rect){
    if(IsLeaf())
        return this;

    for(int i = 0; i < 4; i++)
        if(branches[i]->areaAssignedToNode.AreCoordinatesWithinShape(rect->GetX()-rect->GetHalfOfWidth(), rect->GetY()-rect->GetHalfOfHeight())
            && branches[i]->areaAssignedToNode.AreCoordinatesWithinShape(rect->GetX()+rect->GetHalfOfWidth(), rect->GetY()+rect->GetHalfOfHeight()))
            return (EntityQuadTree*)branches[i]->GetNodeSurroundingRectangle(rect);

    return this;
}
void EntityQuadTree::DivideLeafNode(){
    //Add 4 children serving as cross-sections of this node

    //make sure this node is a leaf
    if(IsLeaf() == false)
        return;

    unsigned int leaf_w = areaAssignedToNode.GetHalfOfWidth();
    unsigned int leaf_h = areaAssignedToNode.GetHalfOfHeight();

    //divide this node into four more nodes
    for(int i = 0; i < 4; ++i){
        branches[i] = new EntityQuadTree();
        branches[i]->SetNodeDimensions(leaf_w, leaf_h);
    }

    unsigned int this_x = areaAssignedToNode.GetX();
    unsigned int this_y = areaAssignedToNode.GetY();

    branches[0]->SetNodeCenter(this_x - areaAssignedToNode.GetHalfOfWidth() * 1 / 2, this_y - areaAssignedToNode.GetHalfOfHeight() * 1 / 2);
    branches[1]->SetNodeCenter(this_x + areaAssignedToNode.GetHalfOfWidth() * 1 / 2, this_y - areaAssignedToNode.GetHalfOfHeight() * 1 / 2);
    branches[2]->SetNodeCenter(this_x + areaAssignedToNode.GetHalfOfWidth() * 1 / 2, this_y + areaAssignedToNode.GetHalfOfHeight() * 1 / 2);
    branches[3]->SetNodeCenter(this_x - areaAssignedToNode.GetHalfOfWidth() * 1 / 2, this_y + areaAssignedToNode.GetHalfOfHeight() * 1 / 2);

    leaf = false;
}
void EntityQuadTree::BlitQuadTree(unsigned int level){
    if(!IsLeaf())
        for(int i = 0; i < 4; i++)
            ((EntityQuadTree*)branches[i])->BlitQuadTree(level+1);

    int start = 32;
    int diff = 128;    
    if(entities.size() > 0)
        LibraryInterface::DrawRect(areaAssignedToNode.GetX()+1, areaAssignedToNode.GetY()+1, areaAssignedToNode.GetW()/2-2, areaAssignedToNode.GetH()/2-2, 255, 0, 0);
    else
        LibraryInterface::DrawRect(areaAssignedToNode.GetX(), areaAssignedToNode.GetY(), areaAssignedToNode.GetW()/2, areaAssignedToNode.GetH()/2, 0, start+level*diff, 0);
    LibraryInterface::DrawCircle(areaAssignedToNode.GetX(), areaAssignedToNode.GetY(), 0, start+level*diff, start+level*diff, 0);
    LibraryInterface::Print(GetEntityCountInNode(),areaAssignedToNode.GetX()-areaAssignedToNode.GetHalfOfWidth(), areaAssignedToNode.GetY()-areaAssignedToNode.GetHalfOfHeight()+level*10-10);
    
}

bool EntityQuadTree::BranchesAreEmpty(){
	for(int i = 0; i < 4; i++){
		if(((EntityQuadTree*)branches[i])->IsLeaf())
			if(((EntityQuadTree*)branches[i])->GetEntityCountInNode() > 0)
				return false;
	}
	return true;
}
void EntityQuadTree::SetChanged(bool changed){
	this->changed = changed;
}
bool EntityQuadTree::HasChanged(){
	return changed;
}
void EntityQuadTree::UniteLeafNodes(){
	
	//combine the cross-sections (children) of this node into one leaf
	for(int i = 0; i < 4; ++i){
		for(int j = 0; j < ((EntityQuadTree*)branches[i])->GetEntityCountInNode(); j++)
			updateQueue.push_back(((EntityQuadTree*)branches[i])->entities.at(j));
		delete branches[i];
		branches[i] = NULL;
	}
	leaf = true;
	
}